//This is the program of the first version for the humanoid robot.


#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include "action.h"
#include "define.h"
#include "error.h"


uint * dj_counts; // The pointer will point to the action data arrays
uchar action;
uchar dj;
unsigned long total_counts;

void high(uchar port)
{
	switch(port)
	{
		case 0:
			PORTA |=1;
			break;
		case 1:
			PORTA |=1<<1;
			break;
		case 2:
			PORTA |=1<<2;
			break;
		case 3:
			PORTA |=1<<3;
			break;
		case 4:
			PORTA |=1<<4;
			break;
		case 5:
			PORTA |=1<<5;
			break;
		case 6:
			PORTA |=1<<6;
			break;
		case 7:
			PROTA |=1<<7;
			break;
		case 8:
			PORTC |=1;
			break;
		case 9:
			PORTC |=1<<1;
			break;
		case 10:
			PORTC |=1<<2;
			break;
		case 11:
			PROTC |=1<<3;
			break;
		case 12:
			PROTC |=1<<4;
			break;
		case 13:
			PROTC |=1<<5;
			break;
		case 14:
			PROTC |=1<<6;
			break;
		case 15:
			PROTC |=1<<7;
			break;
	}
}


void low(uchar port)
{
	switch(port)
	{
		case 0:
			PORTA &=~(1);
			break;
		case 1:
			PORTA &=~(1<<1);
			break;
		case 2:
			PORTA &=~(1<<2);
			break;
		case 3:
			PORTA &=~(1<<3);
			break;
		case 4:
			PORTA &=~(1<<4);
			break;
		case 5:
			PORTA &=~(1<<5);
			break;
		case 6:
			PORTA &=~(1<<6;
			break;
		case 7:
			PROTA &=~(1<<7);
			break;
		case 8:
			PORTC &=~(1);
			break;
		case 9:
			PORTC &=~(1<<1);
			break;
		case 10:
			PORTC &=~(1<<2);
			break;
		case 11:
			PROTC &=~(1<<3);
			break;
		case 12:
			PROTC &=~(1<<4);
			break;
		case 13:
			PROTC &=~(1<<5);
			break;
		case 14:
			PROTC &=~(1<<6);
			break;
		case 15:
			PROTC &=~(1<<7);
			break;
	}
}


void port_init(void)
{
 PORTA = 0x00;
 DDRA  = 0xFF;
 PORTB = 0x01;
 DDRB  = 0x1B;
 PORTC = 0x00;
 DDRC  = 0xFF;
 PORTD = 0x00;
 DDRD  = 0x00;
}

//TIMER1 initialize - prescale:64
// WGM: 0) Normal, TOP=0xFFFF
// desired value: 250000Hz
// actual value: 250000.000Hz (0.0%)
void timer1_init(void)
{
 TCCR1B = 0x00; //stop

 TCCR1A = 0x00;
 TCCR1B = 0x03; //start Timer
}

ISR(TIMER1_COMPA_vect)
{
 //compare occured TCNT1=OCR1A
 low(dj);

 dj++;
 if(dj==DJ_NUM)
 {
 	dj=0;
 	if(total_counts<PWM_CYCLES)
 	{
 		_delay_us((PWM_CYCLES-total_counts)*4);
 	}
 	total_counts=0;
 }

 high(dj);

 total_counts+=TCNT1;
 TCNT1=0x0000;

 OCR1A=dj_counts[dj];

}

ISR(TIMER1_OVF_vect)
{
 //TIMER1 has overflowed

}

ISR(INT0_vect)
{
 //external interupt on INT0
}

ISR(INT1_vect)
{
 //external interupt on INT1
}

//call this routine to initialize all peripherals
void init_devices(void)
{
 //stop errant interrupts until set up
 cli(); //disable all interrupts
 port_init();
 timer1_init();

 MCUCR = 0x00;
 GICR  = 0xC0;
 TIMSK = 0x14; //timer interrupt sources
 sei(); //re-enable interrupts
 //all peripherals are now initialized
}

void choose_action()
{
	cli();
	action=STANDUP;//Here it is fixed to STANDUP,latter it will be determined by the command.

	switch(action)
	{
		case STANDUP:
			dj_counts=standup;
			break;

		default:
			cli();
			while(1)
				error(COMMAND_NOT_FOUND);
	}
}

void take_action()
{
	dj=0;
	TCNT1=0x0000;
	total_counts=0;
	OCR1A=dj_counts[dj];
	high(dj);

	sei();
}
int main()
{
	init_devices();

	_delay_ms(500);

	while(1)
	{
		choose_action();

		take_action();
	}

    return 0;
}


